 //MEP Divenav
	//(c) FH Aachen 2009

	//file: 		adis1635x.c
	//purpose:	define constants for analog devices adis1635x
	//			6DOF sensor via SPI
	//uses:		SPI Routines

#include <stdio.h>
#include <avr/io.h>
#include <avr/wdt.h>
#include <stdlib.h>
#include <util/delay.h>
#include <math.h>
#include <avr/pgmspace.h>
#define LCD_DDR DDRB
#include "../global_types.h"
#include "spidummy.h"
#include "../d072/d072.h"
#include "adis1635x.h"
#include "pressure.h"


//***************************************************************************************
// Write Word for Adis
//***************************************************************************************
void SpiWriteWordAdis(bytes16bits Instruction)
{
	//bytes16bits rc;
	SPDR=Instruction.a[0];			//Write MSB of Instruction
	while(!(SPSR & (1<<SPIF))){};
	SPDR=Instruction.a[1];			//Write LSB of Instruction
	while(!(SPSR & (1<<SPIF))){};

}


//***************************************************************************************
// Read Word for Adis
//***************************************************************************************
bytes16bits SpiReadWordAdis(bytes16bits Instruction)
{
	SPI_MODE_ADIS
	bytes16bits rc;
	SPDR=Instruction.a[0];			//Write the Address of the register to read
	while(!(SPSR & (1<<SPIF))){};
	rc.a[1] = SPDR;					//Store MSB of Answer
	SPDR=Instruction.a[1];			//Write zeros (Dont't care values)
	while(!(SPSR & (1<<SPIF))){};
	rc.a[0] = SPDR;					//Store LSB of Answer
	return(rc);
}



//***************************************************************************************
// Initialize Adis
//***************************************************************************************
uint8_t InitAdis(void)
{
/*
 *	Initialize Adis with following steps
 *		Reset
 *		Write to following Registers: Sample Period, X,Y & Z Accel Offset,X,Y & Z Gyro Offset,
 *			Dynamic Range, Global Command
 *		Check the Status Register
 *
 *	Return Error type (if there is any)
 */

// Reset
	CS_ADIS
	_delay_ms(1);
	PORTC &= ~(1<<PC1);     // pull reset line
	_delay_ms(1);
	PORTC |= (1<<PC1);     // rise reset line
	CD_ADIS

_delay_ms(150);	//Initial Power Up Starting Time
SPI_MODE_ADIS

// Write SMPL_PRD
	CS_ADIS
	//SpiWriteWord(B16(10110111,00000000));	//Higher byte SMPL_PRD
     SpiWriteByte (B8(10110111));
	 SpiWriteByte (B8(00000000));
	CD_ADIS
	_delay_us(ADIS_STALL_INIT);
	CS_ADIS
	//SpiWriteWord(B16(10110110,00000111)); //Lower byte SMPL_PRD	-> this is default & minimum allowable setting
    SpiWriteByte (B8(10110110));
	SpiWriteByte (B8(00000001));

	CD_ADIS
	_delay_us(ADIS_STALL_INIT);


// XACCL_OFF
	CS_ADIS
	SpiWriteByte (B8(10100001));
	SpiWriteByte (B8(00000000));
	CD_ADIS
	_delay_us(ADIS_STALL_INIT);
	CS_ADIS
	SpiWriteByte (B8(10100000));
	SpiWriteByte (B8(00000000));
	CD_ADIS
	_delay_us(ADIS_STALL_INIT);

// YACCL_OFF
	CS_ADIS
	SpiWriteByte (B8(10100011));
	SpiWriteByte (B8(00000000));
	CD_ADIS
	_delay_us(ADIS_STALL_INIT);
	CS_ADIS
	SpiWriteByte (B8(10100010));
	SpiWriteByte (B8(00000000));
	CD_ADIS
    _delay_us(ADIS_STALL_INIT);

// ZACCL_OFF
	CS_ADIS
	SpiWriteByte (B8(10100101));
	SpiWriteByte (B8(00000000));
	CD_ADIS
	_delay_us(ADIS_STALL_INIT);
	CS_ADIS
	SpiWriteByte (B8(10100100));
	SpiWriteByte (B8(00000000));
	CD_ADIS
	_delay_us(ADIS_STALL_INIT);

// XGYRO_OFF
	CS_ADIS
	SpiWriteByte (B8(10011011));
	SpiWriteByte (B8(00000000));
	CD_ADIS
	_delay_us(ADIS_STALL_INIT);
	CS_ADIS
	SpiWriteByte (B8(10011010));
	SpiWriteByte (B8(00000000));
	CD_ADIS
	_delay_us(ADIS_STALL_INIT);

// YGYRO_OFF
	CS_ADIS
	SpiWriteByte (B8(10011101));
	SpiWriteByte (B8(00000000));
	CD_ADIS
	_delay_us(ADIS_STALL_INIT);
	CS_ADIS
	SpiWriteByte (B8(10011100));
	SpiWriteByte (B8(00000000));
	CD_ADIS
	_delay_us(ADIS_STALL_INIT);

// ZGYRO_OFF
	CS_ADIS
	SpiWriteByte (B8(10011111));
	SpiWriteByte (B8(00000000));
	CD_ADIS
	_delay_us(ADIS_STALL_INIT);
	CS_ADIS
	SpiWriteByte (B8(10111100));
	SpiWriteByte (B8(00000000));
	CD_ADIS
	_delay_us(ADIS_STALL_INIT);

// DYN RANG AND FILTER
	CS_ADIS
	SpiWriteByte (B8(10111001));
	SpiWriteByte (B8(00000100));
	CD_ADIS
	_delay_us(ADIS_STALL_INIT);
	CS_ADIS
	SpiWriteByte (B8(10111000));
	SpiWriteByte (B8(00000110));
	CD_ADIS
	_delay_us(ADIS_STALL_INIT);


//Global command
	CS_ADIS
	SpiWriteByte (B8(10111111));
	SpiWriteByte (B8(00000000));
	CD_ADIS
	_delay_us(ADIS_STALL_INIT);
	CS_ADIS
	SpiWriteByte (B8(10111110));
	SpiWriteByte (B8(00000000));
	CD_ADIS
    _delay_us(ADIS_STALL_INIT);

//Check the Status Register
	bytes16bits Instruction;
	bytes16bits Status;

	Instruction.a[1]=0;
	Instruction.a[0]=0x3c;
	CS_ADIS
	Status = SpiReadWordAdis(Instruction);	//Status contains answer  fr. prev. instruc.
	CD_ADIS
	_delay_us(ADIS_STALL_INIT);
	CS_ADIS
	Status = SpiReadWordAdis(Instruction);	//Status contains answer  fr. above's instruc
	CD_ADIS

if((Status.a[0] & B8(11111111))==0)	//Checking if there is any error bit set.
	{
	return(ADIS_NO_ERROR);
	}
else
	{
		if(Status.a[0]==ADIS_ERROR_POWER_BELOW){
		return(ADIS_ERROR_POWER_BELOW);
		}
		else if(Status.a[0]==ADIS_ERROR_POWER_ABOVE){
		return(ADIS_ERROR_POWER_ABOVE);
		}
		else if(Status.a[0]==ADIS_ERROR_CONTROL_REG){
		return(ADIS_ERROR_CONTROL_REG);
		}
		else if(Status.a[0]==ADIS_ERROR_SPI){
		return(ADIS_ERROR_SPI);
		}
		else if(Status.a[0]==ADIS_ERROR_SENSOR_OVERRANGE){
		return(ADIS_ERROR_SENSOR_OVERRANGE);
		}
		else if(Status.a[0]==ADIS_ERROR_SELFTEST){
		return(ADIS_ERROR_SELFTEST);
		}
		else
		return(ADIS_ERROR);
	}
}

//*****************************************************************************************
// Get Acceleration for Adis
//*****************************************************************************************
uint8_t AdisGetAcceleration(bytes16bits * pAdis16bit, bytes16bits * pAdisRegister)
{
	//SPI_MODE_ADIS
	//_delay_us(ADIS_STALL);		//Vivek:Important to have delay at begining since this subroutine is called at the end of a chip deselect command
	CS_ADIS
	*pAdis16bit = SpiReadWordAdis(*pAdisRegister);
	CD_ADIS
	if((pAdis16bit->a[1] & B8(01000000))==0)	//Check error flag is set or not
	{
		pAdis16bit->val &= B16(00111111,11111111);
		if((!(pAdis16bit->val & B16(00110000,00000000))==0)) //Check if the value is negative
			{
	   		pAdis16bit->val |= B16(11000000,00000000);	//Make the value negative for 16 bits format
			}
	return(ADIS_NO_ERROR);
	}
	else
	pAdis16bit->val = 8000;	//In case of error, make result = 8000 (unreachable value)
	return(ADIS_ERROR);

}

uint8_t AdisGetAccelerationSingle(bytes16bits * pAdis16bit, bytes16bits * pAdisRegister)
{
	SPI_MODE_ADIS
	_delay_us(ADIS_STALL);		//Vivek:Important to have delay at begining since this subroutine is called at the end of a chip deselect command
	CS_ADIS
	SpiWriteByte(pAdisRegister->a[0]);
	SpiWriteByte(pAdisRegister->a[1]);
	CD_ADIS
	_delay_us(ADIS_STALL);
	CS_ADIS
	pAdis16bit->a[1] = SpiReadByte();
	pAdis16bit->a[0] = SpiReadByte();
	CD_ADIS
	if((pAdis16bit->a[1] & B8(01000000))==0)	//Check error flag is set or not
	{
		pAdis16bit->val &= B16(00111111,11111111);
		if((!(pAdis16bit->val & B16(00110000,00000000))==0)) //Check if the value is negative
			{
	   		pAdis16bit->val |= B16(11000000,00000000);	//Make the value negative for 16 bits format
			}
	return(ADIS_NO_ERROR);
	}
	else
	pAdis16bit->val = 8000;	//In case of error, make result = 8000 (unreachable value)
	return(ADIS_ERROR);

}


uint8_t AdisGetData(struct_data_set *psDataSet)
{
    uint8_t rc0=0,rc1=0,rc2=0,rc3,rc4,rc5,rc6=0;
	bytes16bits Register;

    SPI_MODE_ADIS
	Register.a[1] = B8(00000000);
	Register.a[0] =0x0b;	//Address Trans-x
	rc0 = AdisGetAcceleration(&(psDataSet->AccelTrans.x), &Register);

	ADIS_STALL1

	Register.a[1] = B8(00000000);
	Register.a[0] =0x0d;	//Address Trans-y
	rc1 = AdisGetAcceleration(&(psDataSet->AccelTrans.x), &Register);

	ADIS_STALL2

	Register.a[0] = 0x0f;	//Address Trans-z
	rc2 = AdisGetAcceleration(&(psDataSet->AccelTrans.y), &Register);
   	ADIS_STALL3

	Register.a[0] = 0x05;	//Address Rot-x
	rc3 = AdisGetAcceleration(&(psDataSet->AccelTrans.z), &Register);

    ADIS_STALL4
	Register.a[0] =0x07;	//Address Rot-y
	rc4 = AdisGetAcceleration(&(psDataSet->AccelRot.x), &Register);

	ADIS_STALL5
	Register.a[0] = 0x09;	//Address Rot-z
	rc5 = AdisGetAcceleration(&(psDataSet->AccelRot.y), &Register);

	ADIS_STALL6
	Register.a[0] = 0x0b;	//Address Trans-x
	rc6 = AdisGetAcceleration(&(psDataSet->AccelRot.z), &Register);

	CD_ADIS
	if((rc1 || rc2 || rc3||rc4||rc5||rc6)==0)	//means if there is no error
	return(ADIS_NO_ERROR);
	else
	return(ADIS_ERROR);

}

//*******************************************************************
//New Subroutine to sample data from ADIS and ADC0
//*******************************************************************

uint8_t SampleSensorData(struct_data_set *psDataSet)
{
    uint8_t rc0=0,rc1=0,rc2=0,rc3,rc4,rc5,rc6,rc7=0;
	bytes16bits Register;

    SPI_MODE_ADIS
	Register.a[1] = B8(00000000);
	Register.a[0] =0x0b;	//Addresss Trans-x
	rc0 = AdisGetAcceleration(&(psDataSet->AccelTrans.x), &Register);

    rc7=WritePressure_16Bit();//ADIS_STALL 1

	Register.a[1] = B8(00000000);
	Register.a[0] =0x0d;	//Address Trans-y
	rc1 = AdisGetAcceleration(&(psDataSet->AccelTrans.x), &Register);

    if(rc7!=0)
	{
		GetPressureData(psDataSet);     //ADIS_STALL2
    	rc7=0;
	}
	

	Register.a[0] = 0x0f;	//Address Trans-z
	rc2 = AdisGetAcceleration(&(psDataSet->AccelTrans.y), &Register);
   	
	
	rc7=WritePressure_16Bit();//ADIS_STALL 3

	Register.a[0] = 0x05;	//Address Rot-x
	rc3 = AdisGetAcceleration(&(psDataSet->AccelTrans.z), &Register);
    
	if(rc7!=0)
	{
		GetPressureData(psDataSet);//ADIS_STALL4
    	rc7=0;

	}     

	Register.a[0] =0x07;	//Address Rot-y
	rc4 = AdisGetAcceleration(&(psDataSet->AccelRot.x), &Register);

	rc7=WritePressure_16Bit();//ADIS_STALL 5

	Register.a[0] = 0x09;	//Address Rot-z
	rc5 = AdisGetAcceleration(&(psDataSet->AccelRot.y), &Register);

	if(rc7!=0)
	{
		GetPressureData(psDataSet); //ADIS_STALL6
    	rc7=0;

	}     

	Register.a[0] = 0x0b;	//Address Trans-x
	rc6 = AdisGetAcceleration(&(psDataSet->AccelRot.z), &Register);

	CD_ADIS
	if((rc1 || rc2 || rc3||rc4||rc5||rc6||rc7)==0)	//means if there is no error
	return(ADIS_NO_ERROR);
	else
	return(ADIS_ERROR);
	
 }
